#include "Vector3D.h"

#include <assert.h>
#include <cmath>

using namespace SG::Algebra;

// #include "../../Common/include/Constant.h"

// using SG::DataStructure::Common::Real;
// using SG::DataStructure::Common::TOLERANCE;
// using SG::Algebra::Vector3D;

namespace SG
{
    namespace Algebra
    {
        Vector3D Cross (const Vector3D& a, const Vector3D& b)
        {
            Vector3D c;
            c.m_x = a.m_y * b.m_z - a.m_z * b.m_y;
            c.m_y = a.m_z * b.m_x - a.m_x * b.m_z;
            c.m_z = a.m_x * b.m_y - a.m_y * b.m_x;

            return c;
        }

        Real_t dot (const Vector3D& a, const Vector3D& b)
        {
            return (a.m_x * b.m_x + a.m_y * b.m_y + a.m_z * b.m_z);
        }

        bool operator== (const Vector3D& a, const Vector3D& b)
        {
            if (fabs (a.m_x - b.m_x) > TOLERANCE || fabs (a.m_y - b.m_y) > TOLERANCE || fabs (a.m_z - b.m_z) > TOLERANCE)
            {
                return false;
            }

            return true;
        }
    }  // namespace SG::Algebra
}

Vector3D& Vector3D::Normalize (Real_t& norm)
{
    norm = m_x * m_x + m_y * m_y + m_z * m_z;
    norm = std::sqrt (norm);
    assert (norm > 1.0e-12);  // 防止被0除
    m_x /= norm;
    m_y /= norm;
    m_z /= norm;

    return *this;
}

Vector3D& Vector3D::Normalize ()
{
    Real_t norm;
    return this -> Normalize (norm);
}

Vector3D Vector3D::Rotate (const Matrixd& T) const
{
    // 检验变换矩阵维度
    assert (3 == T.GetColSize () && 3 == T.GetRowSize ());

    /** v1 = T * v0 */
    return Vector3D{ T (0, 0) * m_x + T (0, 1) * m_y + T (0, 2) * m_z,
                     T (1, 0) * m_x + T (1, 1) * m_y + T (1, 2) * m_z,
                     T (2, 0) * m_x + T (2, 1) * m_y + T (2, 2) * m_z };
}

Real_t Vector3D::operator() (const int i) const
{
    assert ((i > -1) && (i < 3));

    if (0 == i)
    {
        return m_x;
    }

    if (1 == i)
    {
        return m_y;
    }

    // if (2 == i)
    return m_z;
}

Vector3D& Vector3D::operator*= (Real_t factor)
{
    m_x *= factor;
    m_y *= factor;
    m_z *= factor;

    return *this;
}

Vector3D Vector3D::operator* (Real_t factor) const
{
    Vector3D result;
    result.m_x = m_x * factor;
    result.m_y = m_y * factor;
    result.m_z = m_z * factor;

    return result;
}

Vector3D& Vector3D::operator+= (const Vector3D& b)
{
    m_x += b.m_x;
    m_y += b.m_y;
    m_z += b.m_z;

    return *this;
}

Vector3D Vector3D::operator+ (const Vector3D& b) const
{
    Vector3D result;
    result.m_x = m_x + b.m_x;
    result.m_y = m_y + b.m_y;
    result.m_z = m_z + b.m_z;

    return result;
}

Vector3D& Vector3D::operator-= (const Vector3D& b)
{
    m_x -= b.m_x;
    m_y -= b.m_y;
    m_z -= b.m_z;

    return *this;
}

Vector3D Vector3D::operator- (const Vector3D& b) const
{
    Vector3D result;
    result.m_x = m_x - b.m_x;
    result.m_y = m_y - b.m_y;
    result.m_z = m_z - b.m_z;

    return result;
}

bool Vector3D::IsZero () const
{
    if (fabs (m_x) > TOLERANCE || fabs (m_y) > TOLERANCE || fabs (m_z) > TOLERANCE)
    {
        return false;
    }

    return true;
}

bool Vector3D::IsUnit () const
{
    return fabs(m_x*m_x + m_y*m_y + m_z*m_z - 1.0) < 1.0e-12;
}